% tests correction to possible error in GPC code
% The problem is with the covariance of the predictive distribution
% C_* = Sigma_{**} - Q_*EQ_* + Q_*ER(Sum_Ec)^{-1}R'EQ_*
% The error in the orignal GP book is in the calculation of the last term:
%     A = Q_*ER(Sum_Ec)^{-1}R'EQ_*
% Edwin V. Bonilla 
% Last update: 19/08/2011

%clear all;

Nclass = 3; 
N = 5;

Q = blkdiag(rand(N,1), rand(N,1),rand(N,1));


K = blkdiag(rand(N),rand(N),rand(N));
K = K*K';
D = diag(rand(N*Nclass,1));

R = repmat(eye(N),Nclass,1);

% E = inv(K+inv(D));
E      = zeros(N*Nclass,N*Nclass);
sumE   = zeros(N, N);
for i = 1 : Nclass
 idx        = ((i-1)*N+1:i*N)';
 Kc         = K(idx,idx);
 Dc         = D(idx,idx);
 I_N        = eye(N);
 L          = chol(I_N + sqrt(Dc)*Kc*sqrt(Dc))'; % Matlab's chol returns L'
 Ec         = sqrt(Dc)*(L'\(L\sqrt(Dc)));
 E(idx,idx) = Ec;
 sumE       = sumE + Ec;
end
M           = chol(sumE)'; %Matlab's chol returns M'


%A_true      = Q'*E*R*inv(sumE)*R'*E*Q;
A_true      = Q'*E*R*( M'\(M\(R'*E*Q)) );
A_book      = zeros(size(A_true));
A_corrected = zeros(size(A_true));

for i = 1 : Nclass
    idx1 = ((i-1)*N+1:i*N)'; 
    k1    = Q(idx1,i); 
    Ec1   = E(idx1,idx1);
    
    b           = Ec1*k1;
    c_book      = Ec1*(M'\(M\b)); % THIS SHOULD NOT INCLUDE EC1 AS B DOES
    c_corrected =  (M'\(M\b)); % 
    
    for j = 1 : Nclass
        idx2 = ((j-1)*N+1:j*N)'; 
        k2   = Q(idx2,j); 
        Ec2 = E(idx2,idx2);
                
        A_book(i,j)      =  c_book'*k2; % THIS SHOULD INCLUDE EC2
        A_corrected(i,j) =  c_corrected'*Ec2*k2;
        
    end
end

A_true
A_book
A_corrected